from PyQt5.QtWidgets import QAction, QMessageBox
from PyQt5.QtGui import QIcon, QImage, qRgb
import numpy as np
import cv2
import math
# from vm_test import simplify_points
import operator

gray_color_table = [qRgb(i, i, i) for i in range(256)]

def bounding_rect(points):
    xmin = min(points[0])
    xmax = max(points[0])
    ymin = min(points[1])
    ymax = max(points[1])
    return (xmin, ymin, xmax, ymax)

def create_action(icon_path, submenu, parent_widget, shortcut, tip, menu, toolbar):
    the_action = QAction(QIcon(icon_path), submenu, parent_widget)
    the_action.setShortcut(shortcut)
    the_action.setStatusTip(tip)
    the_action.setToolTip(tip)
    menu.addAction(the_action)
    toolbar.addAction(the_action)
    return the_action

def npndarray2qimage(image, copy=False):
    if image is None:
        return QImage()
    if image.dtype == np.float64:
        image = np.uint8(image)
    if image.dtype == np.uint8:
        if len(image.shape) == 2:
            qim = QImage(image.data, image.shape[1], image.shape[0], image.strides[0], QImage.Format_Indexed8).rgbSwapped()
            qim.setColorTable(gray_color_table)
            return qim.copy() if copy else qim
        elif len(image.shape) == 3:
            if image.shape[2] == 3:
                qim = QImage(image.data, image.shape[1], image.shape[0], image.strides[0], QImage.Format_RGB888).rgbSwapped()
                return qim.copy() if copy else qim
            elif image.shape[2] == 4:
                qim = QImage(image.data, image.shape[1], image.shape[0], image.strides[0], QImage.Format_ARGB32).rgbSwapped()
                return qim.copy() if copy else qim

def convex_area(contour):
    return cv2.contourArea(cv2.convexHull(contour))

def _distance_2D(a, b):
    return math.sqrt((a[0]-b[0])**2+(a[1]-b[1])**2)

def regularize_output_approx(approx, image_width, image_height, bVisualized, toleratedMinDistCoeff=0.1):
    res = []
    firstPointDist = []
    for i in range(1, len(approx)):
        distance = _distance_2D(approx[0][0], approx[i][0])
        firstPointDist.append(distance)
        if bVisualized:
            print('distance between 0 and %d point is %f'%(i, distance))
    firstPointDistSorted = sorted(firstPointDist)
    if len(approx) > 4:
        toleratedMinDist = toleratedMinDistCoeff*firstPointDistSorted[-3]
        if bVisualized:
            print('toleratedMinDist is %f'%toleratedMinDist)
        for idx, firstdist in enumerate(firstPointDist):
            if firstdist < toleratedMinDist:
                approx = np.delete(approx, idx+1, 0)
        flag = [0 for i in range(len(approx))]
        for i in range(1, len(approx)):
            if flag[i] == 1:
                continue
            for j in range(i+1, len(approx)):
                if _distance_2D(approx[i][0], approx[j][0]) < toleratedMinDist:
                    flag[j] = 1
        red = []
        for i in range(len(approx)):
            if flag[i] == 0:
                red.append(approx[i])
        approx = np.array(red)
    if bVisualized:
        print('red has %d points: '%len(approx), end='')
        for a in approx:
            print('(%d, %d), '%(a[0][0], a[0][1]), end='')
        print()
    if len(approx) == 4:
        res.append(approx[0])
        approxFirstPointDist = []
        for i in range(3):
            approxFirstPointDist.append(_distance_2D(approx[0][0], approx[i+1][0]))
        approxFirstPointDistMaxIndex = approxFirstPointDist.index(max(approxFirstPointDist))
        approxFirstPointDistMinIndex = approxFirstPointDist.index(min(approxFirstPointDist))
        approxFirstPointDistMidIndex = 3-approxFirstPointDistMaxIndex-approxFirstPointDistMinIndex
        res.append(approx[approxFirstPointDistMinIndex+1])
        res.append(approx[approxFirstPointDistMaxIndex+1])
        res.append(approx[approxFirstPointDistMidIndex+1])
    elif len(approx) == 3:
        triAngleCos = []
        for i in range(3):
            triAngleCos.append(((approx[i][0][0]-approx[(i+2)%3][0][0])*(approx[i][0][0]-approx[(i+1)%3][0][0])+(approx[i][0][1]-approx[(i+2)%3][0][1])*(approx[i][0][1]-approx[(i+1)%3][0][1]))/(_distance_2D(approx[i][0], approx[(i+2)%3][0])*_distance_2D(approx[i][0], approx[(i+1)%3][0])))
        cosMinIndex = triAngleCos.index(min(triAngleCos))
        res.append(approx[cosMinIndex])
        res.append(approx[(cosMinIndex+1)%3])
        caledForthPointX = approx[(cosMinIndex+2)%3][0][0]+approx[(cosMinIndex+1)%3][0][0]-approx[cosMinIndex][0][0]
        caledForthPointY = approx[(cosMinIndex+2)%3][0][1]+approx[(cosMinIndex+1)%3][0][1]-approx[cosMinIndex][0][1]
        if caledForthPointX < 0:
            caledForthPointX = 0
        elif caledForthPointX >= image_width:
            caledForthPointX = image_width-1
        if caledForthPointY < 0:
            caledForthPointY = 0
        elif caledForthPointY >= image_height:
            caledForthPointY = image_height-1
        caledForthPoint = [[caledForthPointX, caledForthPointY]]
        res.append(caledForthPoint)
        res.append(approx[(cosMinIndex+2)%3])
    elif len(approx) > 4:
        disTo0 = []
        k = []
        for i in range(len(approx)):
            disTo0.append(math.sqrt(approx[i][0][0]**2+approx[i][0][1]**2))
            k.append(approx[i][0][1]/approx[i][0][0] if approx[i][0][0] != 0 else 10000)
        disMinIndex = disTo0.index(min(disTo0))
        res.append(approx[disMinIndex])
        disTo0.remove(disTo0[disMinIndex])
        k.remove(k[disMinIndex])
        approx = np.delete(approx, disMinIndex, 0)
        kMaxIndex = k.index(max(k))
        res.append(approx[kMaxIndex])
        disTo0.remove(disTo0[kMaxIndex])
        k.remove(k[kMaxIndex])
        approx = np.delete(approx, kMaxIndex, 0)
        disMaxIndex = disTo0.index(max(disTo0))
        res.append(approx[disMaxIndex])
        disTo0.remove(disTo0[disMaxIndex])
        k.remove(k[disMaxIndex])
        approx = np.delete(approx, disMaxIndex, 0)
        kMinIndex = k.index(min(k))
        res.append(approx[kMinIndex])
    return np.array(res)

def find_polys(image, bVisualized, minAreaCoeff=0.02, approxPolyDPCoeff=0.05, bContourMerged=True, hands_origin=(0,0)):
    polys = []
    contours, hierarchy = cv2.findContours(image, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
    if bVisualized:
        contoursShow = np.zeros((image.shape[0], image.shape[1], 3), dtype='uint8')
        cv2.drawContours(contoursShow, contours, -1, (0, 255, 0))
        cv2.putText(contoursShow, 'found %d contours'%len(contours), (0, 40), cv2.FONT_HERSHEY_COMPLEX, 1, (255, 255, 255), 1)
        cv2.imshow('Contour Show', contoursShow)
        cv2.waitKey()
    min_area = minAreaCoeff*image.shape[0]*image.shape[1]
    if bVisualized:
        print('min_area=%f'%min_area)
    for idx, contour in enumerate(contours):
        convex_esti_area = convex_area(contour)
        if bVisualized:
            print('%dth seeking: %f'%(idx+1, convex_esti_area), end=' ')
        if convex_esti_area < min_area:
            if bVisualized:
                print('the area of the contour is invalid')
            continue
        leng = cv2.arcLength(contour, True)
        approx = cv2.approxPolyDP(contour, leng*approxPolyDPCoeff, True)
        polys.append(approx)
        # if bVisualized:
        #     print('\napprox has %d points, '%len(approx), end ='')
        #     for approx_point in approx:
        #         print('(%d, %d), '%(approx_point[0][0], approx_point[0][1]), end='')
        #     print()
    if bContourMerged:
        boxes = []
        gravities = []
        for app in polys:
            box = cv2.boundingRect(app)
            m = cv2.moments(app)
            boxes.append(box)
            if m['m00']!=0:
                gravities.append((int(m['m10']/m['m00']), int(m['m01']/m['m00'])))
            else:
                gravities.append((int(m['m10']), int(m['m01'])))
        boxes = np.array(boxes)
        keep, overlaped = nms_withOverlaped(boxes, 0.8)
        new_polys = []
        for k, ol in zip(keep, overlaped):
            if len(ol) == 0:
                # polys[k][:, :, 0]+=hands_origin[0]
                # polys[k][:, :, 1]+=hands_origin[1]
                new_polys.append(polys[k])
            else:
                gravity = gravities[k]
                contour = polys[k]
                for o in ol:
                    gravity=((gravity[0]+gravities[o][0])/2,(gravity[1]+gravities[o][1])/2)
                    contour = np.concatenate((contour, polys[o]))
                # contour = arrange_contour_by_gravity(np.unique(contour,axis=0), gravity)
                # contour[:, :, 0]+=hands_origin[0]
                # contour[:, :, 1]+=hands_origin[1]
                new_polys.append(arrange_contour_by_gravity(np.unique(contour,axis=0), gravity))
        polys = new_polys
        #if bVisualized
        if False:
            contourShow = np.stack((image,) * 3, axis=-1)
            for app in polys:
                for a in app:
                    print(a)
                    input()
                    cv2.circle(contourShow, (a[0][0], a[0][1]), 3, (0, 0, 255))
                for i in range(0, len(app)):
                    cv2.line(contourShow, (app[i][0][0], app[i][0][1]), (app[i-1][0][0], app[i-1][0][1]), (0, 0, 255))
            cv2.imshow('contourAndApproxShow', contourShow)
            cv2.waitKey()
        if hands_origin != (0,0):
            for poly in polys:
                poly[:, :, 0]+=hands_origin[0]
                poly[:, :, 1]+=hands_origin[1]
    return np.array(polys)

def arrange_contour_by_gravity(contour, gravity):
    # features = []
    # for point in contour:
    #     xd = point[0][0]-gravity[0]
    #     yd = point[0][1]-gravity[1]
    #     d = math.sqrt(xd**2+yd**2)
    #     features.append((xd/d, yd/d))
    xd=contour[:, :, 0]-gravity[0]
    yd=contour[:, :, 1]-gravity[1]
    d=np.sqrt(xd**2+yd**2)
    features_x = xd/d
    features_y=yd/d
    fx_gt0_inds = np.where(features_x>=0)[0]
    fx_lt0_inds = np.where(features_x<0)[0]
    fx_gt0=contour[fx_gt0_inds]
    fx_lt0=contour[fx_lt0_inds]
    y_sorted_idxs = (-features_y[fx_gt0_inds]).argsort()
    ano_y_sorted_idxs = features_y[fx_lt0_inds].argsort()
    return np.concatenate((fx_gt0[y_sorted_idxs], fx_lt0[ano_y_sorted_idxs]))

def compute_disparity(left, right):
    win_size = 11
    if len(left.shape) == 2:
        channels = 1
    else:
        channels = left.shape[2]
    #matcher_left = cv2.StereoSGBM_create(0, 16*7, 2, 8*2*win_size*win_size, 32*10*win_size*win_size, 8, 31, 1, 8, 8, cv2.STEREO_SGBM_MODE_SGBM_3WAY)
    matcher_left = cv2.StereoSGBM_create(0, 16*8, win_size, 4*channels*win_size*win_size, 32*channels*win_size*win_size, 1, 63, 10, 100, 32, cv2.STEREO_SGBM_MODE_SGBM_3WAY)
    displ = matcher_left.compute(left, right).astype(np.float32)/16.0
    #true_displ = displ.astype(np.uint8)
    matcher_right = cv2.ximgproc.createRightMatcher(matcher_left)
    dispr = matcher_right.compute(left, right).astype(np.float32)/16.0
    #true_dispr = dispr.astype(np.uint8)
    filter = cv2.ximgproc.createDisparityWLSFilter(matcher_left)
    filter.setLambda(3000)
    filter.setSigmaColor(10)
    #filtered_displ = filter.filter(true_displ, left, disparity_map_right=true_dispr)
    filtered_displ = filter.filter(displ, left, disparity_map_right=dispr)
    #filtered_displ/=128
    return filtered_displ

def visualize_disparity(disp):
    # visual_disp = disp.copy()
    # minV = np.min(visual_disp)
    # maxV = np.max(visual_disp)
    # scale = 255/(maxV-minV)
    # visual_disp = (visual_disp-minV)*scale
    # return visual_disp
    disp = cv2.normalize(disp, 0, 255, cv2.NORM_MINMAX)
    return disp

def reproject(disparity, q, bmm, zclamp=[1000, 10000]):
    print(disparity.shape)
    image3d = cv2.reprojectImageTo3D(disparity, q).reshape(disparity.shape[0], disparity.shape[1], 3)
    # if not bmm:
    #     zclamp[0] /= 1000
    #     zclamp[1] /= 1000
    # for r_idx, ir in enumerate(image3d):
    #     for i_idx, i in enumerate(ir):
    #         if i[2] < zclamp[0] or i[2] > zclamp[1]:
    #             image3d[r_idx][i_idx] = np.array([0, 0, 0])
    return image3d

def _distance(p1, p2):
    return math.sqrt((p1[0]-p2[0])**2+(p1[1]-p2[1])**2+(p1[2]-p2[2])**2)

def messageBox_info(msg, parent):
    message = QMessageBox(QMessageBox.Information, 'Information', msg, QMessageBox.Yes, parent)
    message.exec_()

def nms(boxes, threshold=0.9):
    x1 = boxes[:, 0]
    y1 = boxes[:, 1]
    w = boxes[:, 2]
    h = boxes[:, 3]
    area = w*h
    order = y1.argsort()
    keep=[]
    # print(order)
    # input()
    while order.size >0:
        i = order[0]
        keep.append(i)
        if order.size > 0:
            xx1 = np.maximum(x1[i], x1[order[1:]])
            yy1 = np.maximum(y1[i], y1[order[1:]])
            xx2 = np.minimum(x1[i]+w[i], x1[order[1:]]+w[order[1:]])
            yy2 = np.minimum(y1[i]+h[i], y1[order[1:]]+h[order[1:]])
            w1 = np.maximum(0, xx2-xx1+1)
            h1 = np.maximum(0, yy2-yy1+1)
            inter=w1*h1
            ovr = inter/(area[i]+area[order[1:]]-inter)
            inds = np.where(ovr<=threshold)[0]
            order = order[inds+1]
            # print(order, keep)
            # input()
    return keep

def nms_withOverlaped(boxes, threshold=0.9):
    x1 = boxes[:, 0]
    y1 = boxes[:, 1]
    w = boxes[:, 2]
    h = boxes[:, 3]
    area = w*h
    order = np.arange(len(boxes))
    keep=[]
    overlaped=[]
    # print(order)
    # input()
    while order.size >0:
        i = order[0]
        keep.append(i)
        if order.size > 0:
            xx1 = np.maximum(x1[i], x1[order[1:]])
            yy1 = np.maximum(y1[i], y1[order[1:]])
            xx2 = np.minimum(x1[i]+w[i], x1[order[1:]]+w[order[1:]])
            yy2 = np.minimum(y1[i]+h[i], y1[order[1:]]+h[order[1:]])
            w1 = np.maximum(0, xx2-xx1+1)
            h1 = np.maximum(0, yy2-yy1+1)
            inter=w1*h1
            ovr = inter/(area[i]+area[order[1:]]-inter)
            inds = np.where(ovr<threshold)[0]
            overlaped_inds = np.where(ovr >= threshold)[0]
            overlaped.append(order[overlaped_inds+1])
            order = order[inds+1]
            # print(order, keep)
            # input()
    return keep, overlaped

def overlaped_as_size(boxes):
    x1 = boxes[:, 0]
    y1 = boxes[:, 1]
    w = boxes[:, 2]
    h = boxes[:, 3]
    area = w*h
    order = (-area).argsort()
    keep=[]
    overlaped=[]
    overs = []
    # print(order)
    # input()
    while order.size >0:
        i = order[0]
        keep.append(i)
        if order.size > 0:
            xx1 = np.maximum(x1[i], x1[order[1:]])
            yy1 = np.maximum(y1[i], y1[order[1:]])
            xx2 = np.minimum(x1[i]+w[i], x1[order[1:]]+w[order[1:]])
            yy2 = np.minimum(y1[i]+h[i], y1[order[1:]]+h[order[1:]])
            w1 = np.maximum(0, xx2-xx1+1)
            h1 = np.maximum(0, yy2-yy1+1)
            inter=w1*h1
            ovr = inter/(area[i]+area[order[1:]]-inter)
            overlaped_inds = np.where(ovr > 0)[0]
            overlaped.append(order[overlaped_inds+1])
            overs.append(ovr[overlaped_inds])
            order = order[1:]
            # print(order, keep)
            # input()
    return keep, overlaped, overs

def judge_sorted_list_bsimilar(a, b, aw, bw):#a, b需同长
    if len(a) != len(b):
        return False
    dis = 0
    for i in range(len(a)):
        dis+=(a[i]/aw-b[i]/bw)**2
    if dis < 0.5:
        return True
    else:
        return False

def merge_contours(approxes, keep, overlaped):
    gravities = []
    for app in approxes:
        m = cv2.moments(app)
        gravities.append((int(m['m10'] / m['m00']), int(m['m01'] / m['m00'])))
    new_polys = []
    for k, ol in zip(keep, overlaped):
        if len(ol) == 0:
            new_polys.append(approxes[k])
        else:
            gravity = gravities[k]
            contour = approxes[k]
            for o in ol:
                gravity = ((gravity[0] + gravities[o][0]) / 2, (gravity[1] + gravities[o][1]) / 2)
                contour = np.concatenate((contour, approxes[o]))
            new_polys.append(arrange_contour_by_gravity(np.unique(contour,axis=0), gravity))
    return new_polys

def geometry_via_contours(roi, hands_origin=(0,0), bBigOne=False, require=-1):
    polys = []
    roi_shape = roi.shape[:2]
    contours, hierarchy = cv2.findContours(roi, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE)
    approxes, approxes_boxes = simplify_points(contours, min_area=30)
    # keep, overlaped = nms_withOverlaped(approxes_boxes, 0.8)
    # approxes = merge_contours(approxes, keep, overlaped)
    keep, overlaped, overs = overlaped_as_size(approxes_boxes)
    approxes_status = [0 for i in range(len(approxes))]
    if require==-1:
        for idx, i in enumerate(keep):
            if approxes_status[i]:
                continue
            approx = approxes[i]
            overlap = overlaped[idx]
            over = overs[idx]
            leng = cv2.arcLength(approx, True)
            approxPoly = cv2.approxPolyDP(approx, leng * 0.05, True)
            bClosed = judge_closed(approx)
            if len(approxPoly) < 10:
                over_idxs = (-over).argsort()
                poly = [4, []]
                the_underside = None
                if bClosed:
                    for oi in over_idxs:
                        if over[oi] < 0.8 or approxes_status[overlap[oi]]:
                            break
                        if len(np.unique((approx, approxes[overlap[oi]]), axis=0)) < 1.4*len(approx):
                            approxes_status[overlap[oi]] = 1
                            continue
                        the_approx_area = convex_area(approxes[overlap[oi]])
                        if the_approx_area/(approxes_boxes[i][2]*approxes_boxes[i][3])<0.8:
                            continue
                        the_underside = approxes[overlap[oi]]
                        sides = overlaped[keep.index(overlap[oi])]
                        approxes_status[overlap[oi]] =1
                        break
                else:
                    for oi in over_idxs:
                        if over[oi] < 0.8 or approxes_status[overlap[oi]]:
                            break
                        if len(np.unique(np.concatenate((approx, approxes[overlap[oi]])), axis=0)) < 1.4*len(approx):
                            approxes_status[overlap[oi]] = 1
                            continue
                        the_approx_closed = judge_closed(approxes[overlap[oi]])
                        if the_approx_closed:
                            the_approx_area = convex_area(approxes[overlap[oi]])
                            if the_approx_area/(approxes_boxes[i][2]*approxes_boxes[i][3])<0.8:
                                continue
                            the_underside = approxes[overlap[oi]]
                            sides = overlaped[keep.index(overlap[oi])]
                            approxes_status[overlap[oi]] =1
                            break
                        else:
                            approx = merge_contours([approx, approxes[overlap[oi]]], [0], [[1]])
                            the_approx_area = convex_area(approx)
                            overlap = np.delete(np.unique(np.concatenate((overlap, overlaped[keep.index(overlap[oi])])), axis=0), overlap[oi])
                            if the_approx_area / (approxes_boxes[i][2] * approxes_boxes[i][3]) < 0.8:
                                continue
                            the_underside = approx
                            sides = overlap
                            approxes_status[overlap[oi]] = 1
                            break
                if the_underside is None:
                    the_underside = approx
                    sides = overlap
                else:
                    leng = cv2.arcLength(the_underside, True)
                    approxPoly = cv2.approxPolyDP(the_underside, leng*0.05, True)
                # if len(sides) == 0:
                #     sides = np.delete(np.arange(len(approxes)), i)
                side = -1
                for s in reversed(sides):
                    for appP in approxPoly:
                        point_to_contour = cv2.pointPolygonTest(approxes[s], (appP[0][0], appP[0][1]), True)
                        if abs(point_to_contour) <5:
                            side = s
                            break
                    if side >= 0:
                        break
                if side >= 0:
                    yell_idx = []
                    approxes_status[side] = 1
                    for idx, appP in enumerate(approxPoly):
                        point_to_contour = cv2.pointPolygonTest(approxes[side], (appP[0][0], appP[0][1]), True)
                        if abs(point_to_contour) < 5:
                            yell_idx.append(idx)
                    leng = cv2.arcLength(approxes[side], True)
                    side_approxPoly = cv2.approxPolyDP(approxes[side], leng*0.05, True)
                    poly[1].append(approxPoly)
                    underside = np.array([[[-10000, -10000]] for i in range(len(approxPoly))])
                    side_approxPoly_pt_status = [0 for i in range(len(side_approxPoly))]
                    for yi in yell_idx:
                        dis_dis = 0
                        for sai, sa in enumerate(side_approxPoly):
                            if side_approxPoly_pt_status[sai]:
                                continue
                            the_dis_dis = abs(cv2.pointPolygonTest(the_underside, (sa[0][0], sa[0][1]), True))/_distance_2D(approxPoly[yi][0], sa[0])
                            if the_dis_dis > dis_dis:
                                dis_dis = the_dis_dis
                                corresponding_underside_pt_idx = sai
                        if dis_dis >=0.25:
                            underside[yi][0] = side_approxPoly[corresponding_underside_pt_idx][0]
                    poly[1].append(underside)
                    polys.append(poly)
                # for approP in approxPoly:
                #     poly[1].append
                # underside_rect = cv2.boundingRect(the_underside)
            else:
                isEllipse = False
                poly = [6, []]
                if bClosed:
                    isEllipse, points = identify_ellipse(approx)
                if isEllipse:
                    sides = overlap
                    side = -1
                    for s in reversed(sides):
                        for appP in points:
                            point_to_contour = cv2.pointPolygonTest(approxes[s], (appP[0][0], appP[0][1]), True)
                            if abs(point_to_contour) < 5:
                                side = s
                                break
                        if side >=0:
                            break
                    if side >= 0:
                        yell_idx = []
                        approxes_status[side] = 1
                        for idx, appP in enumerate(points):
                            point_to_contour = cv2.pointPolygonTest(approxes[side], (appP[0][0], appP[0][1]), True)
                            if abs(point_to_contour) < 5:
                                yell_idx.append(idx)
                        leng = cv2.arcLength(approxes[side], True)
                        side_approxPoly = cv2.approxPolyDP(approxes[side], leng * 0.05, True)
                        poly[0] = 0
                        poly[1].append(points)
                        underside = np.array([[[-10000, -10000]] for i in range(len(approxPoly))])
                        side_approxPoly_pt_status = [0 for i in range(len(side_approxPoly))]
                        for yi in yell_idx:
                            dis_dis = 0
                            for sai, sa in enumerate(side_approxPoly):
                                if side_approxPoly_pt_status[sai]:
                                    continue
                                the_dis_dis = abs(
                                    cv2.pointPolygonTest(the_underside, (sa[0][0], sa[0][1]), True)) / _distance_2D(
                                    approxPoly[yi][0], sa[0])
                                if the_dis_dis > dis_dis:
                                    dis_dis = the_dis_dis
                                    corresponding_underside_pt_idx = sai
                            if dis_dis >=0.25:
                                underside[yi][0] = side_approxPoly[corresponding_underside_pt_idx][0]
                        poly[1].append(underside)
                        polys.append(poly)
                    else:
                        poly[0] = 1
                        poly[1].append(points)
                        polys.append(poly)
                else:
                    over_idxs = (-over).argsort()
                    poly = [4, []]
                    the_underside = None
                    if bClosed:
                        for oi in over_idxs:
                            if over[oi] < 0.8 or approxes_status[overlap[oi]]:
                                break
                            if len(np.unique((approx, approxes[overlap[oi]]), axis=0)) < 1.4 * len(approx):
                                approxes_status[overlap[oi]] = 1
                                continue
                            the_approx_area = convex_area(approxes[overlap[oi]])
                            if the_approx_area / (approxes_boxes[i][2] * approxes_boxes[i][3]) < 0.8:
                                continue
                            the_underside = approxes[overlap[oi]]
                            sides = overlaped[keep.index(overlap[oi])]
                            approxes_status[overlap[oi]] = 1
                            break
                    else:
                        for oi in over_idxs:
                            if over[oi] < 0.8 or approxes_status[overlap[oi]]:
                                break
                            if len(np.unique(np.concatenate((approx, approxes[overlap[oi]])), axis=0)) < 1.4 * len(approx):
                                approxes_status[overlap[oi]] = 1
                                continue
                            the_approx_closed = judge_closed(approxes[overlap[oi]])
                            if the_approx_closed:
                                the_approx_area = convex_area(approxes[overlap[oi]])
                                if the_approx_area / (approxes_boxes[i][2] * approxes_boxes[i][3]) < 0.8:
                                    continue
                                the_underside = approxes[overlap[oi]]
                                sides = overlaped[keep.index(overlap[oi])]
                                approxes_status[overlap[oi]] = 1
                                break
                            else:
                                approx = merge_contours([approx, approxes[overlap[oi]]], [0], [[1]])
                                the_approx_area = convex_area(approx)
                                overlap = np.delete(
                                    np.unique(np.concatenate((overlap, overlaped[keep.index(overlap[oi])])), axis=0),
                                    overlap[oi])
                                if the_approx_area / (approxes_boxes[i][2] * approxes_boxes[i][3]) < 0.8:
                                    continue
                                the_underside = approx
                                sides = overlap
                                approxes_status[overlap[oi]] = 1
                                break
                    if the_underside is None:
                        the_underside = approx
                        sides = overlap
                    else:
                        leng = cv2.arcLength(the_underside, True)
                        approxPoly = cv2.approxPolyDP(the_underside, leng * 0.05, True)
                    # if len(sides) == 0:
                    #     sides = np.delete(np.arange(len(approxes)), i)
                    side = -1
                    for s in reversed(sides):
                        for appP in approxPoly:
                            point_to_contour = cv2.pointPolygonTest(approxes[s], (appP[0][0], appP[0][1]), True)
                            if abs(point_to_contour) < 5:
                                side = s
                                break
                        if side >= 0:
                            break
                    if side >= 0:
                        yell_idx = []
                        approxes_status[side] = 1
                        for idx, appP in enumerate(approxPoly):
                            point_to_contour = cv2.pointPolygonTest(approxes[side], (appP[0][0], appP[0][1]), True)
                            if abs(point_to_contour) < 5:
                                yell_idx.append(idx)
                        leng = cv2.arcLength(approxes[side], True)
                        side_approxPoly = cv2.approxPolyDP(approxes[side], leng * 0.05, True)
                        poly[1].append(approxPoly)
                        underside = np.array([[[-10000, -10000]] for i in range(len(approxPoly))])
                        side_approxPoly_pt_status = [0 for i in range(len(side_approxPoly))]
                        for yi in yell_idx:
                            dis_dis = 0
                            for sai, sa in enumerate(side_approxPoly):
                                if side_approxPoly_pt_status[sai]:
                                    continue
                                the_dis_dis = abs(
                                    cv2.pointPolygonTest(the_underside, (sa[0][0], sa[0][1]), True)) / _distance_2D(
                                    approxPoly[yi][0], sa[0])
                                if the_dis_dis > dis_dis:
                                    dis_dis = the_dis_dis
                                    corresponding_underside_pt_idx = sai
                            if dis_dis >= 0.25:
                                underside[yi][0] = side_approxPoly[corresponding_underside_pt_idx][0]
                        poly[1].append(underside)
                        polys.append(poly)
                    # for approP in approxPoly:
                    #     poly[1].append
                    # underside_rect = cv2.boundingRect(the_underside)
            approxes_status[i]=1
            if bBigOne and len(polys) >0:
                break
    elif require == 1:
        _, points = identify_ellipse(approxes[keep[0]])
        poly = [1, []]
        poly[1].append(points)
        polys.append(poly)
    if hands_origin != (0,0):
        for poly in polys:
            for p in poly[1]:
                p[:, :, 0]+=hands_origin[0]
                p[:, :, 1]+=hands_origin[1]
    return polys

def judge_closed(contour):
    if _distance_2D(contour[0][0], contour[-1][0]) < 20:
        return True
    else:
        return False

def identify_ellipse(contour, bThinkReallyEllipse=False):
    rect = cv2.minAreaRect(contour)
    if bThinkReallyEllipse or abs(convex_area(contour)/(math.pi*rect[1][0]*rect[1][1]/4)-1)<0.2:
        points = []
        if rect[3] == -90 or rect[3] == 0:
            bDown = bUp = bLeft = bRight = False
            direction_repre = []
            for c in contour:
                if c[0][0] == int(rect[0][0]):
                    if c[0][1] < rect[0][1]:
                        if not bDown:
                            points.append(c)
                            bDown = True
                            direction_repre.append(1)
                    elif c[0][1]>rect[0][1]:
                        if not bUp:
                            points.append(c)
                            bUp = True
                            direction_repre.append(2)
                elif c[0][1] == int(rect[0][1]):
                    if c[0][0] < rect[0][0]:
                        if not bLeft:
                            points.append(c)
                            bLeft = True
                            direction_repre.append(3)
                    elif c[0][0] > rect[0][0]:
                        if not bRight:
                            points.append(c)
                            bRight = True
                            direction_repre.append(4)
            new_points = []
            if _distance_2D(points[direction_repre.index(1)][0], rect[0]) < _distance_2D(points[direction_repre.index(3)][0], rect[0]):
                points_order = [3,4,1,2]
            else:
                points_order = [1,2,3,4]
            for po in points_order:
                new_points.append(points[direction_repre.index(po)])
            points = new_points
        else:
            over_center_dis = []
            over_points = []
            under_center_dis = []
            under_points = []
            for c in contour:
                to_center_dis = _distance_2D(c[0], rect[0])
                if c[0][1]>rect[0][1]:
                    over_center_dis.append(to_center_dis)
                    over_points.append(c)
                else:
                    under_center_dis.append(to_center_dis)
                    under_points.append(c)
            points.append(over_points[over_center_dis.index(max(over_center_dis))])
            points.append(under_points[under_center_dis.index(max(under_center_dis))])
            points.append(over_points[over_center_dis.index(min(over_center_dis))])
            points.append(under_points[under_center_dis.index(min(under_center_dis))])
        even_points = np.concatenate(([points[0]], [points[1]], [points[2]], [points[3]]))
        return True, even_points
    else:
        return False, None

def geometry_via_yolo(out_classes, out_boxes, roi, hands_origin=(0,0)):
    polys = []
    contours, hierarchy = cv2.findContours(roi, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE)
    approxes, approxes_boxes = simplify_points(contours, min_area=30)
    for i, c in reversed(list(enumerate(out_classes))):
        box = out_boxes[i]
        top, left, bottom, right = box
        top = max(0, np.floor(top + 0.5).astype('int32'))
        left = max(0, np.floor(left + 0.5).astype('int32'))
        bottom = min(roi.shape[0], np.floor(bottom + 0.5).astype('int32'))
        right = min(roi.shape[1], np.floor(right + 0.5).astype('int32'))
        if c != 1:
            polies = geometry_via_contours(roi[top:bottom, left:right], (left, top), True)
        else:
            polies = geometry_via_contours(roi[top:bottom, left:right], (left, top), True, 1)
        polys.append(polies[0])
    if hands_origin != (0,0):
        for poly in polys:
            for p in poly[1]:
                p[:, :, 0]+=hands_origin[0]
                p[:, :, 1]+=hands_origin[1]
    return polys

def cal_new_disparity(point, left_marks, right_marks, seek_left_src_roi, seek_right_src_roi):
    dis = 10000
    target_i = 0
    if len(seek_left_src_roi.shape) >2:
        cv2.cvtColor(seek_left_src_roi, cv2.COLOR_BGR2GRAY)
        cv2.cvtColor(seek_right_src_roi, cv2.COLOR_BGR2GRAY)
    seek_left_src_roi = np.pad(seek_left_src_roi, ((1, 1), (1, 1)), 'constant', constant_values=(0, 0))
    seek_right_src_roi = np.pad(seek_right_src_roi, ((1, 1), (1, 1)), 'constant', constant_values=(0, 0))
    left_marks_dis = left_marks[:, 0]-point[0]
    for i in range(point[0]):
        right_marks_dis = right_marks[:, 0]-i
        the_dis = 0
        the_dis+=np.sum((left_marks_dis-right_marks_dis)**2)
        the_dis+=20*np.sum((seek_left_src_roi[point[1]:point[1]+3, point[0]:point[0]+3]-seek_right_src_roi[point[1]:point[1]+3, i:i+3])**2)
        if the_dis < dis:
            dis = the_dis
            target_i = i
    return point[0]-target_i

def S_for_poly(poly, cloud, reproj_origins=(0,0)):
    if len(poly)>2:
        S=0
        # print(poly)
        # input()
        if len(poly.shape) == 4:
            poly = poly.reshape((poly.shape[0], 1, 2))
        origin_point = poly[0]
        for i in range(1, len(poly)-1):
           S+=triangle_S_by_nodes_3d(cloud[reproj_origins[1]+origin_point[0][1]][reproj_origins[0]+origin_point[0][0]], cloud[reproj_origins[1]+poly[i][0][1]][reproj_origins[0]+poly[i][0][0]], cloud[reproj_origins[1]+poly[i+1][0][1]][reproj_origins[0]+poly[i+1][0][0]])
        return S
    else:
        return 0

def triangle_S_by_nodes_3d(pt1, pt2, pt3):
    a = _distance(pt1, pt2)
    b=_distance(pt1,pt3)
    c=_distance(pt2, pt3)
    half_c=(a+b+c)/2
    return math.sqrt(half_c*(half_c-a)*(half_c-b)*(half_c-c))

def poly_rect(poly, bHasNeg=False):
    if len(poly.shape) == 4:
        poly = poly.reshape((poly.shape[0], 1, 2))
    if bHasNeg:
        for po in poly:
            if po[0][0]<0 or po[0][1]<0:
                poly = np.delete(poly, po)
    # print(poly)
    xmin = np.min(poly[:, :, 0])
    xmax = np.max(poly[:, :, 0])
    ymin = np.min(poly[:, :, 1])
    ymax = np.max(poly[:, :, 1])
    return (xmin, ymin, xmax, ymax)

def simplify_points(contours, min_area=10, lengCoeff=0.02):
    approxes = []
    approxes_bbox = []
    for contour in contours:
        convex_esti_area = convex_area(contour)
        if convex_esti_area < min_area:
            continue
        # leng = cv2.arcLength(contour, True)
        # approx = cv2.approxPolyDP(contour, leng*lengCoeff, False)
        # approxes.append(approx)
        approxes.append(contour)
        approxes_bbox.append(cv2.boundingRect(contour))
    return approxes, np.array(approxes_bbox)
